package com.sensors;

import android.util.Log;

import com.DataProcess.Robot;
//import com.pudu.mydemo.nativeClass.NativeClass;
import com.pudu.mydemo.nativeClass.NativeClass;
import com.pudu.mydemo.view.rplidar_node_t;
import com.tool.Coordinate;

import java.util.Arrays;
import java.util.List;
import java.util.Vector;

/**
 * Created by tjpu on 2016/10/12.
 */
public class Lidar {
    static String thisLogTag = "Lidar";

    static double ROTATION_ANGLE = -0.100;  //往顺时针矫正为负，往逆时针方向为正
    static double CONVERT_TO_METRIC = 0.001;
    static double CHECK_LEAST_RANG_X = 0.10;
    static double CHECK_LEAST_NUM = 3;
    static double CHECK_TARGET_WEIGHT = 0.8;
    static double CHECK_FORESEE_SPEED = 0.3;
    static double R = Robot.WHEELS_THREAD / 2.0 + 0.1;  //轮子到中心点的距离 + 安全余量

    static Coordinate lidar = new Coordinate(0.19, 0.0);  //机器人中心点为原点，雷达坐标
    static List<Coordinate> metricUnit = new Vector<>();
    static List<ObstaclePoint> obstacleList = new Vector<>();
    static double foreseeTime = 4.0;   //机器人中心点到障碍物的时间
    static double radius = 1.0;
    static double foreseeSpeed = 0.3;
    static double foreseeAngle = foreseeSpeed * foreseeTime / Math.abs(radius); //障碍物检测区域
    private static double closeTime = 10.0;  //机器人前边沿到障碍物的时间

    public static class ObstaclePoint extends Object{
        public double x;
        public double y;
        public double foreseeTime;
    }

    public static double getCloseTime() {
        return closeTime;
    }

    public static void putOrigin(List<rplidar_node_t> lidarData) {
        metricUnit.clear();

        for(int i=0;i<lidarData.size();i++) {
            double[] data = rotation(-lidarData.get(i).ptY, lidarData.get(i).ptX, ROTATION_ANGLE);
            Coordinate point = new Coordinate();
            point.x = data[0] * CONVERT_TO_METRIC + lidar.x;
            point.y = data[1] * CONVERT_TO_METRIC + lidar.y;
            metricUnit.add(point);
        }

        NativeClass.cGetLidarData(metricUnit);

        checkObstacle();
    }

    static void setForeseeRang() {
        double speedLAve;
        double speedRAve;
        if(Robot.targetSpeed[0] * Robot.speed[0] >= 0)   //防止急刹瞬间反向速度太大导致的前瞻区域变成在后面
            speedLAve = (1 - CHECK_TARGET_WEIGHT) * Robot.speed[0] + CHECK_TARGET_WEIGHT * Robot.targetSpeed[0];
        else
            speedLAve = Robot.targetSpeed[0];
        if(Robot.targetSpeed[1] * Robot.speed[1] >= 0)
            speedRAve = (1 - CHECK_TARGET_WEIGHT) * Robot.speed[1] + CHECK_TARGET_WEIGHT * Robot.targetSpeed[1];
        else
            speedRAve = Robot.targetSpeed[1];
        double speedAve = (speedLAve + speedRAve)/2.0;

        if(Math.abs(speedRAve - speedLAve) < 0.00001)
           radius = 1000;
        else
            radius =  (speedLAve + speedRAve) / (speedRAve - speedLAve) * Robot.WHEELS_THREAD;

        if(Math.abs(radius) < 0.005)
            radius = 0.005;

        Log.i(thisLogTag,"lidar speedLAve: "+speedLAve+" turnRadius: "+radius+" speed: "+speedAve);
        if(speedAve >= 0)
            foreseeSpeed = CHECK_FORESEE_SPEED;
        else
            foreseeSpeed = -CHECK_FORESEE_SPEED;

        foreseeAngle = foreseeSpeed * foreseeTime / Math.abs(radius);
    }

    static double[] rotation(double x, double y, double angle) {
        double [] newPoint = new double[2];

        newPoint[0] = Math.cos(angle) * x - Math.sin(angle) * y;
        newPoint[1] = Math.sin(angle) * x + Math.cos(angle) * y;
        return newPoint;
    }

    static void checkObstacle() {

        setForeseeRang();
        obstacleList.clear();
        Coordinate turnRadius = new Coordinate(0,radius);

        for(int i=0; i<metricUnit.size(); i++) {
            if (metricUnit.get(i).x < lidar.x + CHECK_LEAST_RANG_X)
                continue;

            Coordinate point = metricUnit.get(i);
            Coordinate vector = new Coordinate(point.x - turnRadius.x, point.y - turnRadius.y);
            Coordinate vector2 = new Coordinate(0 - turnRadius.x, 0 - turnRadius.y);

            //求出夹角
            double angle = Math.acos((vector.x * vector2.x + vector.y * vector2.y) / (vector.getLength() * vector2.getLength()));
//            Log.i(thisLogTag,"angle: " + angle);

            //判断是否为障碍点
            if (vector.getLength() > (Math.abs(radius) - R)
                    && vector.getLength() < (Math.abs(radius) + R)) {
                if (angle < foreseeAngle) {
                    // 该点在前进线路区域范围内
                    ObstaclePoint obstaclePoint = new ObstaclePoint();
                    obstaclePoint.x = point.x;
                    obstaclePoint.y = point.y;
                    obstaclePoint.foreseeTime = angle * Math.abs(radius) / foreseeSpeed;
                    obstacleList.add(obstaclePoint);
//                    Log.i(thisLogTag,"length: " + vector.getLength() + " angle: " + angle);
//                    Log.i(thisLogTag, "obstacle point X: " + point.x + " Y: " + point.y + " time: " + obstaclePoint.foreseeTime);
                }
            }
        }

        //求出最近障碍物的点的到达时间
        if (obstacleList.size() > CHECK_LEAST_NUM) {

            double[] time = new double[obstacleList.size()];
            for (int i = 0; i < obstacleList.size(); i++) {
                time[i] = obstacleList.get(i).foreseeTime;
            }
            double timeX;
            for (int i = 0; i < time.length - 1; i++) {
                for (int j = i+1; j < time.length; j++) {
                    if (time[i] > time[j]) {
                        timeX = time[j];
                        time[j] = time[i];
                        time[i] = timeX;
                    }
                }
            }
//            Log.i(thisLogTag,"the closest time: "+ Arrays.toString(time));
            closeTime = time[3] - lidar.x / foreseeSpeed;  //到机器人的外壳的最短时间

        } else {
            closeTime = 10.0;
        }
        Log.i(thisLogTag,"the closest time: "+ closeTime);
    }
}
